/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <mex.h>
#include "../voslam/voslam_geo.h"



#define DEG_TO_RAD 0.01745329251994
#define zeros(X,Y) mxCreateDoubleMatrix(X,Y,mxREAL);
#define sendup(X) mxSetName(X,#X); mexPutArray(X,"base")


/* mex -v -Wall relative_transf.c ../voslam/voslam_geo.o ../voslam/voslam_util.o ../voslam/voslam_blas.o*/


/******************************************************/
void mexFunction(
				 int nlhs, mxArray *plhs[],
				 int nrhs, const mxArray *prhs[]
				 )
{
	const mxArray  *mv1,*mv2;
	mxArray	*mv3;	
	int i=0;					 
	
	double *v1,*v2,*v3;

	static int	expectRhs = 2;
	static int	expectLhs = 2;
	char		buffer[100];
	
	/* Check for proper number of arguments */
	
	if (nrhs != expectRhs) 
	{
		sprintf(buffer, "posef_calc requires %d input arguments. Got %d",
			expectRhs, nrhs);
		mexErrMsgTxt(buffer);
	}
	else if (nlhs > expectLhs) 
	{
		sprintf(buffer, "posef_calc requires %d outputs arguments. Got %d",
			expectLhs, nlhs);
		mexErrMsgTxt(buffer);
	}
	
	mv1 = prhs[i++];
	mv2 = prhs[i++];
    
	mv3 = zeros(7,1);

	v1=mxGetPr(mv1);
	v2=mxGetPr(mv2);
	v3=mxGetPr(mv3);

    relative_transf(v1,v2,v3);

	i=0;
	
	plhs[i++]=mv3;
    
}
